#pragma once

#include <boost/cstdint.hpp>
#include <boost/unordered_map.hpp>
#include <boost/function.hpp>
#include <fstream>
#include <iostream>

#include "graph.h"
#include "graph_data.h"
#include "edges_rasterizer.h"

typedef uint32_t bucket_id_t;

struct spatial_index_t
{
  spatial_index_t(graph_t const & graph, size_t lon_count, size_t lat_count);  
  
  void for_all_nearest_edges(vertex_data_t const & pos, boost::function<void (edge_t)> fn) const;
private:
  typedef boost::unordered_multimap<bucket_id_t, edge_t> buckets_map_t;
  
  bucket_id_t get_bucket_id(vertex_data_t pos) const;
  bucket_id_t get_bucket_id(int x, int y) const;
  
  point2_t get_point(geodata_t const & geodata) const;
  void build_index(graph_t const & graph);
  void add_to_index(int x, int y, edge_t const & edge);
  
  size_t          lon_count_;
  size_t          lat_count_;
  buckets_map_t   buckets_map_;
  graph_t const & graph_;
};


struct vector2_t
{
  vector2_t(double x, double y)
    : x_(x), y_(y)
  {     
  }
  
  double get_x() const
  {
    return x_;
  }
  
  double get_y() const
  {
    return y_;
  }
  
  void set_x(double x)
  {
    x_ = x;
  }
  
  void set_y(double y)
  {
    y_ = y;    
  }  
  
  double length() const
  {
    return std::sqrt(x_ * x_ + y_ * y_);
  }
  
  vector2_t & operator *= (double n)
  {
    x_ *= n;
    y_ *= n;
    return *this;
  }
  
  vector2_t & operator /= (double n)
  {
    x_ /= n;
    y_ /= n;
    return *this;
  }
private:
  double x_;
  double y_;  
};

inline double scalar_product(vector2_t const & v1, vector2_t const & v2)
{
  return v1.get_x() * v2.get_x() + v1.get_y() * v2.get_y();
}

namespace 
{
  double const PI =3.14159265358979323846;
  
  inline double deg_to_rad(double deg)
  {
    return deg * PI / 180.;
  }
  
  inline double rad_to_deg(double rad)
  {
    return rad * 180.0 / PI;
  }
  
  point2_t to_mercator(point2_t const & pt)
  {
    return point2_t(pt.x(), 
                    std::log(std::tan(PI / 4.0 + deg_to_rad(pt.y()) / 2.0)));        
  }
  
  point2_t from_mercator(point2_t const & pt)
  {
    return point2_t(pt.x(), 
                    rad_to_deg(2.0 * std::atan(std::exp(pt.y())) - PI / 2.0));
  }
}// namespace

inline std::pair<double, vertex_data_t> get_dist(vertex_data_t pos, vertex_data_t edge_start, vertex_data_t edge_end)
{
  point2_t start = to_mercator(point2_t(edge_start.lon, edge_start.lat));
  point2_t end   = to_mercator(point2_t(edge_end.lon, edge_end.lat));
  point2_t point = to_mercator(point2_t(pos.lon, pos.lat));
  
  vector2_t e(end.x() - start.x(), end.y() - start.y());
  vector2_t a(point.x() - start.x(), point.y() - start.y());
    
  double proj = scalar_product(e, a) / e.length();
  std::cout << start.x() << " " << start.y() << std::endl;
  
  if (proj <= 0.0)
  {
    return std::make_pair(orthodromic_distance(pos, edge_start), edge_start);    
  }
  else if (proj >= e.length())
  {
    return std::make_pair(orthodromic_distance(pos, edge_end), edge_end);
  }
  else
  {
    e *= proj / e.length();

    point2_t projection_point = from_mercator(point2_t(e.get_x() + start.x(), e.get_y() + start.y()));

    vertex_data_t data(projection_point.x(), projection_point.y());
    return std::make_pair(orthodromic_distance(data, pos), data);
  }
}

// inline std::pair<double, vertex_data_t> get_dist(vertex_data_t pos, vertex_data_t edge_start, vertex_data_t edge_end)
// {
//   vector2_t e(edge_end.lon - edge_start.lon, edge_end.lat - edge_start.lat);
//   vector2_t a(pos.lon - edge_start.lon, pos.lat - edge_start.lat);
//     
//   double proj = scalar_product(e, a) / e.length();
//   if (proj <= 0.0)
//   {
//     return std::make_pair(orthodromic_distance(pos, edge_start), edge_start);    
//   }
//   else if (proj >= e.length())
//   {
//     return std::make_pair(orthodromic_distance(pos, edge_end), edge_end);
//   }
//   else
//   {
//     e *= proj / e.length();
// 
//     vertex_data_t point(e.get_x() + edge_start.lon, e.get_y() + edge_start.lat);
//     return std::make_pair(orthodromic_distance(point, pos), point);
//   }
// }
